import rospy
import tf
from geometry_msgs.msg import PointStamped
import numpy as np

class PointTransform():
    def __init__(self):
        self.listener = tf.TransformListener()
        self.rot = None
        self.trans = None
        self.get_transform("panda_link0", "camera_color_optical_frame")

    def get_transform(self, from_frame, to_frame):

        self.listener.waitForTransform(from_frame, to_frame, rospy.Time(0), rospy.Duration(3))
        (trans, rot) = self.listener.lookupTransform(from_frame, to_frame, rospy.Time(0))
        self.trans = trans
        self.rot = rot

    def transformation(self,point):
  
        rotation_matrix = tf.transformations.quaternion_matrix(self.rot)[:3, :3]

        transform_matrix = np.eye(4)
        transform_matrix[:3, :3] = rotation_matrix 
        transform_matrix[:3, 3] = self.trans

        input_point = np.concatenate([point, [1]]) 
        transformed_point = np.dot(transform_matrix, input_point) 
        
        return transformed_point[:3]

if __name__ == '__main__':
    rospy.init_node('point_transform_node')

    point_transform = PointTransform()

    print(point_transform.transformation([0,0,1]))
